#include "bezier3.h"
#include "controller/height_controller.h"
#include <algorithm>

const float Bezier3::bezier_matrix[4][4]  = { 	{-1.0f, 3.0f,-3.0f ,1.0f},
												{3.0f,-6.0f,3.0f,0.0f},
												{-3.0f,3.0f,0.0f,0.0f},
												{1.0f,0.0f,0.0f,0.0f}};

Bezier3::Bezier3(){
	memcpy(this->matrix,Bezier3::bezier_matrix,16*sizeof(float));
}

Bezier3::~Bezier3(){}

void Bezier3::calculatePoints(){
	buffer.clear();
	for(int g=0;g<((int)control.size());g+=4){
		for(unsigned int u=0;u<=Bezier3::step;u++){
			buffer.push_back(CurveModel::calculatePoint((float)u/(float)Bezier3::step,(float*)&control[g],stride));
		}
	}
}

Point Bezier3::calculatePoint(float u){
	float segments = control.size()/4;

	if(u>segments)
		return Point();

	float nu = u-(unsigned int)u;
	unsigned int segment = (unsigned int)u;

	return CurveModel::calculatePoint(nu,(float*)&control[(segment)*4],stride);
}

